#include "cmsis_os2.h"
#include "stdio.h"
#include "hi_stdlib.h"
#include "hi_uart.h"
#include "hi_errno.h"
#include "hi_io.h"
#include "hi_gpio.h"
#include "hi_task.h"

#include "servo.h"
#include "uart.h"

#define GPIO_0_UART1_TXD 2
#define GPIO_1_UART1_RXD 2
#define P -0.474
#define D -0.2

//屏幕长约为1900，宽约为1100

static void *UartTask(char *param)
{
    unsigned char uartBuff[UART_BUFF_SIZE] = {0};
    unsigned int bboxBuff[4] = {0};
    static int lastx = 0;
    static int x = 0;
    static int err = 0;
    int duty = 0;

    hi_io_set_func(HI_IO_NAME_GPIO_0, GPIO_0_UART1_TXD);
    hi_io_set_func(HI_IO_NAME_GPIO_1, GPIO_1_UART1_RXD);

    while (1)
    {
        memset_s(uartBuff, UART_BUFF_SIZE, 0, UART_BUFF_SIZE);
        int ret = 0;
        ret = hi_uart_read(HI_UART_IDX_1, uartBuff, UART_BUFF_SIZE);
        if (ret > 0)
        {
            hi_u8 length = uartBuff[0];
            if (length + 1 == ret)
            {
                memcpy_s(bboxBuff, sizeof(bboxBuff), uartBuff + 1, sizeof(bboxBuff));
                x = bboxBuff[0] + bboxBuff[2] / 2;
                err = x - lastx;
                printf("[INFO] ");
                for (int i = 0; i < 4; i++)
                {
                    printf("%d, ", bboxBuff[i]);
                }
                printf("\n");
            }
            else
            {
                printf("[ERROR] ret{%d} is not equal to length{%d}.\n", ret, length);
            }
        }

        //控制外设
        printf("err : %d\n", err);
        Servo_Send_PWM(P * x + D * err + 1950);
        lastx = x;
        hi_sleep(20); /* 20:sleep 20ms */
    }
    return NULL;
}

void UartTransmit(hi_void)
{
    int ret = 0;

    hi_uart_attribute uartAttr = {
        .baud_rate = 115200,
        .data_bits = 8,
        .pad = 0,
        .parity = 0,
        .stop_bits = 1};

    hi_uart_extra_attr extraAttr = {0};
    extraAttr.rx_block = HI_UART_BLOCK_STATE_NONE_BLOCK;
    extraAttr.tx_block = HI_UART_BLOCK_STATE_NONE_BLOCK;

    ret = hi_uart_init(HI_UART_IDX_1, &uartAttr, &extraAttr);
    if (ret != 0)
    {
        printf("[ERROR] Failed to init uart! Err code = %d\n", ret);
        return;
    }

    /* Create a task to handle uart communication */
    osThreadAttr_t attr = {0};
    attr.name = "uart";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 1024;
    attr.priority = osPriorityNormal;
    if (osThreadNew((osThreadFunc_t)UartTask, NULL, &attr) == NULL)
    {
        printf("[ERROR] Failed to create uart demo task!\n");
    }
}